/*
 *  lib-smv: Matrix - Vector Library targeted to Structural Mechanics Problems
 *  Copyright (C) 2006-2008  St.Brcic, Lj.Zugic
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 
/** @file TQLI.cpp
 *  @brief Implementation of class TQLI - solution of the eigenvalue problem
 *
 *  EIGENVALUE PROBLEMS:  Class TQLI   
 *  STANDARD EIGENVALUE PROBLEMS A*x = L*x
 *  
 *  Householder transformation to tridiagonal form
 *  QL transformation with implicit shifts
 */ 
//!  Class TQLI
/*! ================================================= TQLI.cpp
//
//                   EIGENVALUE ANALYSIS
//               STANDARD EIGENVALUE PROBLEMS
//
//                      A*x = L*x 
//
//  Class TQLI
//  Householder transformation to tridiagonal form
//  QL transformation with implicit shifts
//  
//  (mainly according to "Numerical Recipes in C")
//
//============================================================*/
//

#include "TQLI.h"
namespace smv_lib {

//! empty (default) constructor
TQLI::TQLI(void) {
    isMatrix_A   = false;
    isCalculated = false;
    isVName      = false;
    isdName      = false;
}

//! constructor for the standard problem
TQLI::TQLI(Matrix& a) : A(a) {
    N = A.getRows();
    assert(N == A.getCols());
    isMatrix_A   = true;
    isCalculated = false;
    isVName      = false;
    isdName      = false;
}

//! destructor
TQLI::~TQLI(void) {}

//! set Matrix A (for the standard problem)
void TQLI::setMatrix_A(Matrix& a) {
    A = a;
    N = a.getRows();
    assert(N == a.getCols());
    isMatrix_A   = true;
    isCalculated = false;
    isVName      = false;
    isdName      = false;
}

void TQLI::setV_Name(string vname) {
    V_Name  = vname;
    isVName = true;
}

void TQLI::setd_Name(string dname) {
    d_Name  = dname;
    isdName = true;
}

//! solve the standard eigenvalue problem
void TQLI::calculate() {
    if(isMatrix_A == false) {
        cerr << "\nTQLI: Matrix A is NOT given!\n";
        return;
    }
    //---
    Vector e(N);
    if (isVName == false)
        V_Name = "H_Modal.txt";
    Matrix Modal(N,N,V_Name);
    if(isdName == false)
        d_Name = "H_Spec.txt";
    Vector Spektar(N,d_Name);
    V = Modal;
    d = Spektar;
    //--- Householder reduction and tqli
    tred2(e);
    tqli(e);
    //--- sort eigenvectors and eigenvalues
    eigsrt_tqli();
    isCalculated = true;

    //--- write obtained results to files
    V.setName(V_Name);
    V.Write();
    d.Write();
    cout << "\nEigen-Vectros are in the file H_MODAL.txt\n";
    cout << "\nEigen-Values are in the file H_SPEC.txt\n";
}

//! give the modal matrix
Matrix& TQLI::giveModalMatrix() {
    Matrix* Zero = new Matrix(N,N);
    if(isCalculated == true)
        return V;
    else
        return (*Zero);
}

//! give eigenvalues
Vector& TQLI::giveSpectrum() {
    Vector* zero = new Vector(N);
    if(isCalculated == true)
        return d;
    else
        return (*zero);
}

//=========== STANDARD EIGENVALUE PROBLEMS  ----------------- private functions
//            (complete solution)
//----------- Householder reduction
//---------- tred2
//-----------------------------------------------------------------------
/**
**   \brief tred2: HOUSEHOLDER REDUCTION of a real symmetric Matrix A(n,n)
**
**   HOUSEHOLDER REDUCTION of a real symmetric Matrix A(n,n)
**   to tridiagonal form by n-2 orthogonal transformations.
**   INPUT:
**   Matrix A(n,n) - the real symmetrix matrix of order n
**   OUTPUT:
**   -Matrix A(n,n) is replaced by orthogonal matrix Q,
**   -Vector d(n) returns diagonal elements of tridiagonal matrix,
**   -Vector e(n) returns the off-diagonal elements with e[1]=0.
**    NOTE; Matrix A and Vector d are private membrs of the clas TQLI
**
**   Householder reduction TRED2 combined with TQLI is the MOST
**   EFFICIENT technique for finding all the eigenvalues and
**   eigenvectors of a real symmetric matrix. ("NR in C")
**/
//---------------------------------------------------------------------
void TQLI::tred2 (Vector& e) {
    //--------- local variables
    int L, k, j, i;
    double scale, hh, h, g, f;
    int n = A.getRows();
    double A_ij, A_ik, A_ki, A_jk, A_kj;    // elements of Matrix A
    double temp;

    //--------- algorithm begins (according to "NR in C", p.373)
    for (i=n-1; i>0; i--)  {        // n-2 transformations
        L = i - 1;
        h = scale = 0.0;
        if (L > 0)  {
            for (k=0; k<=L; k++) {
                A_ik = A.getElement(i,k);
                scale += fabs(A_ik);
            }
            if (scale == 0.0)
                e.setData(i, A.getElement(i,L));
            else  {
                for (k=0; k<=L; k++)  {
                    A_ik = A.getElement(i,k);
                    A_ik /= scale;
                    A.setElement(i,k, A_ik);   // A(i,k) /= scale
                    A_ik = A.getElement(i,k);
                    h += A_ik * A_ik;
                }
                f = A.getElement(i,L);
                g = (f >= 0 ? -sqrt(h) : sqrt(h));
                e.setData(i, scale * g);
                h -= f * g;
                A.setElement(i,L, (f - g));
                f = 0.0;
                for (j=0; j<=L; j++)  {
                    A_ij = A.getElement(i,j);
                    A.setElement(j,i, A_ij / h);
                    g = 0.0;
                    for (k=0; k<=j; k++) {
                        A_ik = A.getElement(i,k);
                        A_jk = A.getElement(j,k);
                        g += A_jk * A_ik;
                    }
                    for (k=j+1; k<=L; k++) {
                        A_ik = A.getElement(i,k);
                        A_kj = A.getElement(k,j);
                        g += A_kj * A_ik;
                    }
                    e.setData(j, g / h);
                    f += e.getData(j) * A.getElement(i,j);
                } // end for (j=0; j <L j++)
                hh = f /(h+h);
                for (j=0; j<=L; j++)  {
                    f = A.getElement(i,j);
                    g = e.getData(j) - hh * f;
                    e.setData(j,g);
                    for (k=0; k<=j; k++) {
                        A_ik = A.getElement(i,k);
                        A_jk = A.getElement(j,k);
                        temp = (f * e.getData(k) + g * A_ik);
                        A_jk -= temp;
                        A.setElement(j,k, A_jk);
                    }
                }
            }
        } // end if (L>0)
        else {
            e.setData(i, A.getElement(i,L));
        }
        d.setData(i,h);
    }  // end for (i=n-1; i0; i--)

    d.setData(0, 0.0);
    e.setData(0, 0.0);
    for (i=0; i<n; i++)  {
        L = i - 1;
        if (d.getData(i))  {  // block is skipped if i = 0: d(0) = 0.0
            for (j=0; j<=L; j++)  {
                g = 0.0;
                for (k=0; k<=L; k++) {
                    A_ik = A.getElement(i,k);
                    A_kj = A.getElement(k,j);
                    g += A_ik * A_kj;
                }
                for (k=0; k<=L; k++) {
                    A_ki = A.getElement(k,i);
                    A_kj = A.getElement(k,j);
                    A_kj -= g * A_ki;
                    A.setElement(k,j, A_kj);
                }
            }  // end for (j=0;..
        } // end if (d[i])
        d.setData(i, A.getElement(i,i));
        A.setElement(i,i, 1.0);
        for (j=0; j<=L; j++)  {
            A.setElement(j,i, 0.0);
            A.setElement(i,j, 0.0);
        }
    }  // end for(i=0; i<n; i++);
}
//
//---------- tqli
//--------------------------------------------------------------------
/**
** \brief tqli: QL ALGORITHM WITH IMPLICIT SHIFTS 
**
**   QL ALGORITHM WITH IMPLICIT SHIFTS 
**   to determine the eigenvalues and eigenvectors
**   of a real, symmetric, tridiagonal matrix, (isTred2 == false) or 
**   of a real, symmetric matrix previously reduced by tred2 (isTred2 == true)
**
**   INPUT:
**   -Vector d(n) with diagonal elements of tridiagonal matrix,
**   -Vector e(n) with off-diagonal elements with e[0]=0.
**
**   NOTE: Vectors d and e are usually output results from TRED2
**         In that case, Matrix z is the output Matrix A from TRED2
**
**   OUTPUT:
**   -Vector d(n) - eigenvalues
**   -Matrix V(n,n) - eigenvectors (columns of V)
**
**   Note: Eigenvalues and eigenvectors should be later sorted by eigsrt
**   Householder reduction TRED2 combined with TQLI is the MOST
**   EFFICIENT technique for finding all the eigenvalues and
**   eigenvectors of a real symmetric matrix. ("NR in C")
**/
//---------------------------------------------------------------------
void TQLI::tqli (Vector& e, bool isTred2) {
    int n = d.getLen();
    //---- local variables
    int m,L,iter,i,k;
    double s,r,p,g,f,dd,c,b;
    double d_m, d_m1, d_L, d_L1, d_i1;   // elements of Vector d
    double e_m, e_L;                     // elements of Vector e
    double z_ki;                         // elements of Matrix V
    double temp, temp1;

    if (isTred2 == false) // in the case when tridiagomal matrix is input
        V.Identity();
    else
        V = A;            // in the case of input from tred2
    for (i=1; i<n; i++)
        e.setData(i-1, e.getData(i));
    e.setData(n-1, 0.0);

    for (L=0; L<n; L++) {
        iter=0;
        do {
            for (m=L; m<(n-1); m++) {
                d_m  = d.getData(m);
                d_m1 = d.getData(m+1);
                dd   = fabs(d_m) + fabs(d_m1);
                e_m  = e.getData(m);
                if ((double)(fabs(e_m) + dd) == dd)
                    break;
            }
            if (m != L) {
                assert (iter++ <= 30);
                d_L  = d.getData(L);
                d_L1 = d.getData(L+1);
                d_m  = d.getData(m);
                e_L  = e.getData(L);
                g    = (d_L1 - d_L)/(2.0 * e_L);
                r    = pythag(g, 1.0);
                g    = d_m - d_L + e_L/(g+SIGN(r,g));
                s    = c = 1.0;
                p    = 0.0;
                for (i=m-1; i>=L; i--) {
                    f = s * e.getData(i);
                    b = c * e.getData(i);
                    r = pythag(f,g);
                    e.setData(i+1, r);
                    if (r == 0.0) {
                        d_i1 = d.getData(i+1);
                        d_i1 -= p;
                        d.setData(i+1, d_i1);
                        e.setData(m, 0.0);
                        break;
                    } // end if (r == 0.0)
                    s = f / r;
                    c = g / r;
                    g = d.getData(i+1) - p;
                    r = (d.getData(i)-g)*s + 2.0*c*b;
                    p = s * r;
                    d.setData(i+1, g+p);
                    g = c * r - b;
                    /* Next loop can be omitted if the eigenvectors are not wanted*/
                    for (k=0; k<n; k++) {
                        f     = V.getElement(k,i+1);
                        z_ki  = V.getElement(k,i);
                        temp  = s*z_ki + c*f;
                        temp1 = c*z_ki - s*f;
                        V.setElement(k,i+1, temp);
                        V.setElement(k,i, temp1);
                    }
                }  // end for (i=m-1; i>=L; i--)
                if (r == 0.0 && i >= L)
                    continue;
                d_L  = d.getData(L);
                d_L -= p;
                d.setData(L, d_L);
                e.setData(L, g);
                e.setData(m, 0.0);
            }  // end if (m != L)
        } while (m != L);        // end do - while
    }  // end for (L=0; L<n; L++)
}

//================ eigsrt
//------------------------------------------------------------------------------
/**
**  \brief eigsrt: sorting eigenvalues/eigenvectors in ascending order
**
**  INPUT:
**  Unsorted eigenvalues are in Vector d(n) and
**  corresponding eigenvector matrix is V(n,n)
**  (as obtained from JACOBI or TQLI)
**  OUTPUT:
**  Sorted into ascending order eigenvalues in Vector d(n) and
**  correspondingly sorted eigenvector matrix V(n,n)
**  Note: Vector d(n) ans Matrix V(n,n) are the private members of TQLI class
**/
//-------------------------------------------------------------------------------
void TQLI::eigsrt_tqli() {
    int k, j, i;
    double p;
    int n = d.getLen();

    for (i=0; i<n-1; i++)  {
        k = i;
        p = d.getData(k);               // p = d(k)
        for (j=i+1; j<n; j++) {
            if (d.getData(j) <= p) {    // d(j) <= p
                k = j;
                p = d.getData(k);       // p = d(k)
            }
        }   // end for j = i+1 ...
        if (k != i)  {
            d.setData(k,d.getData(i));   // d(k) = d(i)
            d.setData(i,p);                // d(i) = p
            for (j=0; j<n; j++)  {
                p = V.getElement(j,i);                  // p = V(j,i)
                V.setElement(j,i,V.getElement(j,k));    // V(j,i) = V(j,k)
                V.setElement(j,k,p);                    // V(j,k) = p
            }
        }  // end for k != i
    }   // end for i = 0 ...
}

//
//===================== EXTERNAL EIGENVALUE ROUTINES
//
//=========== STANDARD EIGENVALUE PROBLEMS
//            (complete solution)
//
//---------- tred2
//-------------------------------------------------------------------------------------------
/**
**   \brief tred2: HOUSEHOLDER REDUCTION of a real symmetric Matrix A(n,n) - EXTERNAL function
**
**   HOUSEHOLDER REDUCTION of a real symmetric Matrix A(n,n)
**   to tridiagonal form by n-2 orthogonal transformations.
**   INPUT:
**   Matrix A(n,n) - the real symmetrix matrix of order n
**   OUTPUT:
**   -Matrix A(n,n) is replaced by orthogonal matrix Q,
**   -Vector d(n) returns diagonal elements of tridiagonal matrix,
**   -Vector e(n) returns the off-diagonal elements with e[1]=0.
**
**   Householder reduction TRED2 combined with TQLI is the MOST
**   EFFICIENT technique for finding all the eigenvalues and
**   eigenvectors of a real symmetric matrix. ("NR in C")
**/
//--------------------------------------------------------------------------------------------
void tred2 (Matrix& A, Vector& d, Vector& e) {
    //--------- local variables
    int L, k, j, i;
    double scale, hh, h, g, f;
    int n = A.getRows();
    assert(n == A.getCols());

    d.setSize(n);
    e.setSize(n);
    double A_ij, A_ik, A_ki, A_jk, A_kj;    // elements of Matrix A
    double temp;

    for (i=n-1; i>0; i--)  {
        L = i - 1;
        h = scale = 0.0;
        if (L > 0)  {
            for (k=0; k<=L; k++) {
                A_ik = A.getElement(i,k);
                scale += fabs(A_ik);
            }
            if (scale == 0.0)
                e.setData(i,A.getElement(i,L));
            else  {
                for (k=0; k<=L; k++)  {
                    A_ik = A.getElement(i,k);
                    A_ik /= scale;
                    A.setElement(i,k,A_ik);   // A(i,k) /= scale
                    A_ik = A.getElement(i,k);
                    h += A_ik * A_ik;
                }
                f = A.getElement(i,L);
                g = (f >= 0 ? -sqrt(h) : sqrt(h));
                e.setData(i, scale * g);
                h -= f * g;
                A.setElement(i,L, (f - g));
                f = 0.0;
                for (j=0; j<=L; j++)  {
                    A_ij = A.getElement(i,j);
                    A.setElement(j,i, A_ij / h);
                    g = 0.0;
                    for (k=0; k<=j; k++) {
                        A_ik = A.getElement(i,k);
                        A_jk = A.getElement(j,k);
                        g += A_jk * A_ik;
                    }
                    for (k=j+1; k<=L; k++) {
                        A_ik = A.getElement(i,k);
                        A_kj = A.getElement(k,j);
                        g += A_kj * A_ik;
                    }
                    e.setData(j, g / h);
                    f += e.getData(j) * A.getElement(i,j);
                } // end for (j=0; j <L j++)

                hh = f /(h+h);
                for (j=0; j<=L; j++)  {
                    f = A.getElement(i,j);
                    g = e.getData(j) - hh * f;
                    e.setData(j,g);
                    for (k=0; k<=j; k++) {
                        A_ik = A.getElement(i,k);
                        A_jk = A.getElement(j,k);
                        temp = (f * e.getData(k) + g * A_ik);
                        A_jk -= temp;
                        A.setElement(j,k,A_jk);
                    }
                }
            }
        } // end if (L>0)
        else {
            e.setData(i, A.getElement(i,L));
        }
        d.setData(i,h);
    }

    d.setData(0, 0.0);
    e.setData(0, 0.0);
    for (i=0; i<n; i++)  {
        L = i - 1;
        if (d.getData(i))  {    // block is skipped if i = 0: d(0) = 0.0
            for (j=0; j<=L; j++)  {
                g = 0.0;
                for (k=0; k<=L; k++) {
                    A_ik = A.getElement(i,k);
                    A_kj = A.getElement(k,j);
                    g += A_ik * A_kj;
                }
                for (k=0; k<=L; k++) {
                    A_ki = A.getElement(k,i);
                    A_kj = A.getElement(k,j);
                    A_kj -= g * A_ki;
                    A.setElement(k,j, A_kj);
                }
            }  // end for (j=0;..
        } // end if (d[i])
        d.setData(i, A.getElement(i,i));
        A.setElement(i,i, 1.0);
        for (j=0; j<=L; j++)  {
            A.setElement(j,i, 0.0);
            A.setElement(i,j, 0.0);
        }
    }
}
//
//---------- tqli
//--------------------------------------------------------------------
/**
** \brief tqli: QL ALGORITHM WITH IMPLICIT SHIFTS - EXTERNAL function 
**
**   QL ALGORITHM WITH IMPLICIT SHIFTS 
**   to determine the eigenvalues and eigenvectors
**   of a real, symmetric, tridiagonal matrix, (isTred2 == false) or 
**   of a real, symmetric matrix previously reduced by tred2 (isTred2 == true)
**
**   INPUT:
**   -Vector d(n) with diagonal elements of tridiagonal matrix,
**   -Vector e(n) with off-diagonal elements with e[0]=0
**   -bool isTred2 (true for tqli used after tred2)
**
**   NOTE: Vectors d and e are usually output results from TRED2
**         In that case, Matrix Z is the output Matrix A from TRED2
**
**   OUTPUT:
**   -Vector d(n) - eigenvalues
**   -Matrix Z(n,n) - eigenvectors (columns of Z)
**
**   Note: Eigenvalues and eigenvectors should be later sorted by eigsrt
**   Householder reduction TRED2 combined with TQLI is the MOST
**   EFFICIENT technique for finding all the eigenvalues and
**   eigenvectors of a real symmetric matrix. ("NR in C")
**/
//---------------------------------------------------------------------
void tqli (Vector& d, Vector& e, Matrix& Z, bool isTred2) {
    int n = d.getLen();
    Z.setRows(n);
    Z.setCols(n);
    //---- local variables
    int m,L,iter,i,k;
    double s,r,p,g,f,dd,c,b;
    double d_m, d_m1, d_L, d_L1, d_i1;   // elements of Vector d
    double e_m, e_L;                     // elements of Vector e
    double z_ki;                         // elements of Matrix Z
    double temp, temp1;

    if (isTred2 == false)  // if isTred2 == true => matrix Z is output from tred2
        Z.Identity();      // else, Z is identity matrix
    for (i=1; i<n; i++)
        e.setData(i-1, e.getData(i));
    e.setData(n-1, 0.0);

    for (L=0; L<n; L++) {
        iter=0;
        do {
            for (m=L; m<n-1; m++) {
                d_m  = d.getData(m);
                d_m1 = d.getData(m+1);
                dd   = fabs(d_m) + fabs(d_m1);
                e_m  = e.getData(m);
                if ((double)(fabs(e_m) + dd) == dd)
                    break;
            }
            if (m != L) {
                assert(iter++ <= 30);
                d_L  = d.getData(L);
                d_L1 = d.getData(L+1);
                d_m  = d.getData(m);
                e_L  = e.getData(L);
                g    = (d_L1 - d_L)/(2.0 * e_L);
                r    = pythag(g, 1.0);
                g    = d_m - d_L + e_L/(g+SIGN(r,g));
                s    = c = 1.0;
                p    = 0.0;
                for (i=m-1; i>=L; i--) {
                    f = s * e.getData(i);
                    b = c * e.getData(i);
                    r = pythag(f,g);
                    e.setData(i+1, r);
                    if (r == 0.0) {
                        d_i1 = d.getData(i+1);
                        d_i1 -= p;
                        d.setData(i+1, d_i1);
                        e.setData(m, 0.0);
                        break;
                    }
                    s = f / r;
                    c = g / r;
                    g = d.getData(i+1) - p;
                    r = (d.getData(i)-g)*s + 2.0*c*b;
                    p = s * r;
                    d.setData(i+1, g+p);
                    g = c * r - b;
                    /* Next loop can be omitted if the eigenvectors are not wanted*/
                    for (k=0; k<n; k++) {
                        f     = Z.getElement(k,i+1);
                        z_ki  = Z.getElement(k,i);
                        temp  = s*z_ki + c*f;
                        temp1 = c*z_ki - s*f;
                        Z.setElement(k,i+1, temp);
                        Z.setElement(k,i, temp1);
                    }
                }
                if (r == 0.0 && i >= L)
                    continue;
                d_L  = d.getData(L);
                d_L -= p;
                d.setData(L, d_L);
                e.setData(L, g);
                e.setData(m, 0.0);
            }
        } while (m != L);
    }
}
//--------- eigsrt
//------------------------------------------------------------------------------------
/**
**  \brief eigsrt: sorting eigenvalues/eigenvectors in ascending order - EXTERNAL function
**
**  INPUT:
**  Unsorted eigenvalues in Vector Lam(n) and
**  corresponding eigenvector matrix X(n,n)
**  (as obtained from JACOBI or TQLI)
**  OUTPUT:
**  Sorted into ascending order eigenvalues in Vector d(n) and
**  correspondingly sorted eigenvector matrix X(n,n)
**/
//----------------------------------------------------------------------------------
void eigsrt_tqli (Vector& d, Matrix& V) {
    int k, j, i;
    double p;
    int n = d.getLen();

    for (i=0; i<n-1; i++)  {
        k = i;
        p = d.getData(k);               // p = d(k)
        for (j=i+1; j<n; j++) {
            if (d.getData(j) <= p) {    // d(j) <= p
                k = j;
                p = d.getData(k);       // p = d(k)
            }
        }
        if (k != i)  {
            d.setData(k,d.getData(i));   // d(k) = d(i)
            d.setData(i,p);             // d(i) = p
            for (j=0; j<n; j++)  {
                p = V.getElement(j,i);                 // p = V(j,i)
                V.setElement(j,i,V.getElement(j,k));    // V(j,i) = V(j,k)
                V.setElement(j,k,p);                   // V(j,k) = p
            }
        } // end for k != i
    }  // end for i = 0 ...
}

//======================== utility function
// Computes (a2 + b2)^1/2 without destructive underflow or overflow
double pythag(double a, double b) {
    double absa,absb;
    absa=fabs(a);
    absb=fabs(b);
    if (absa > absb)
        return absa*sqrt(1.0 + DSQR(absb/absa));
    else
        return (absb == 0.0 ? 0.0 : absb*sqrt(1.0 + DSQR(absa/absb)));
}

}   // end of namespace smv_lib

//========================= end of file
